#include <math/math.h>
#include <limits.h>

namespace yam3d
{
namespace math
{

quat quatFromMat4( const mat4& mat )
{
    float x, y, z, w;
    x = y = z = w = 0.0f;
   // const qreal* m = mat.data();
    qreal m[4][4];

    for( int i=0; i<4; ++i )
    {
        m[i][0] = mat.row(i).x();
        m[i][1] = mat.row(i).y();
        m[i][2] = mat.row(i).z();
        m[i][3] = mat.row(i).w();
    }

    const float trace = m[0][0] + m[1][1] + m[2][2];

    if ( trace > 0.f )
    {
        float root = sqrtf( trace + 1.f );
        w = root * .5f;
        assert( fabsf(root) >= 0.0000001f );
        root = .5f / root;
        x = root * (m[1][2] - m[2][1]);
        y = root * (m[2][0] - m[0][2]);
        z = root * (m[0][1] - m[1][0]);
    }
    else
    {
        register size_t i = 0;
        if ( m[1][1] > m[0][0] )
            i = 1;
        if ( m[2][2] > m[i][i] )
            i = 2;
        const size_t j = (i == 2 ? 0 : i+1);
        const size_t k = (j == 2 ? 0 : j+1);

        float root = sqrtf( m[i][i] - m[j][j] - m[k][k] + 1.f );
        float* v = &x;
        v[i] = root * .5f;
        assert( fabsf(root) >= 0.0000001f );
        root = .5f / root;
        v[j] = root * (m[i][j] + m[j][i]);
        v[k] = root * (m[i][k] + m[k][i]);
        w = root * (m[j][k] - m[k][j]);
    }

    return quat(w,x,y,z);
}

void toAngleAxis( const quat& q, float* angle, vec3* axis )
{
    assert( angle != 0 );
    assert( axis != 0 );

    assert( fabsf(q.length()-1.0f) < 0.00001f ); // normalize q before use

    *angle = 2.f * acosf(q.scalar());
    const float s = sqrtf(1.f-q.scalar()*q.scalar());

    if ( s <= 0.00001f )
    {
            axis->setX( q.x() );
            axis->setY( q.y() );
            axis->setZ( q.z() );
    }
    else
    {
            const float is = 1.f/s;
            axis->setX( q.x() * is );
            axis->setY( q.y() * is );
            axis->setZ( q.z() * is );
    }

}


quat fromAngleAxis( float angle, const vec3& axis )
{
    float s = sinf(angle*.5f);
    float c = cosf(angle*.5f);

    quat q( c, axis.x()*s, axis.y()*s , axis.z()*s );
    return q;
}

vec3 average( const core::vector<vec3>& values )
{
    assert( values.size() > 0 );

    vec3 res(0,0,0);
    for( int i=0; i<values.size(); ++i )
    {
        res+= values[i];
    }

    return res / values.size();
}


quat average( const core::vector<quat>& values )
{
    assert( values.size() > 0 );
    assert( fabs(values[0].length()-1.0f) < 0.00001f );

    if( values.size() == 1 )
    {
        return values[0];
    }

    // because -q == q, therefore "regular average" does not work correctly, if there is
    // almost same orientation with totally opposite axis. Therefore assume first axis of first q
    // to be correct and if there is "opposite axis" (i.e. dot, product < 0 i.e. angle between two axis > 90)
    // afterwards, use -q instead of q.

    quat res = values[0];

    float angleOld(0.0f);
    math::vec3 axisOld(0,0,0);
    math::toAngleAxis(values[0], &angleOld, &axisOld );

    for(int i=1; i<values.size(); ++i )
    {
        assert( fabs(values[i].length()-1.0f) < 0.00001f );
        float angleNew(0.0f);
        math::vec3 axisNew(0,0,0);
        math::toAngleAxis(values[i], &angleNew, &axisNew );

        if( math::vec3::dotProduct(axisNew,axisOld) > 0.0f )
        {
            res += values[i];
        }
        else
        {
            res += -values[i];
        }
    }

    res.normalize();
    return res;
}



}
}
